#include "controlacc.h"

ControlAcc::ControlAcc()
{
}

ControlAcc::~ControlAcc()
{
}

void ControlAcc::Config(int v0, int vMax, int toMaxUs)
{
    m_v0 = v0; 
    m_vMax = vMax;
    m_toMaxUs = toMaxUs;
    m_currentV = v0;
    m_speed = 1.0 * (m_vMax - m_v0) / toMaxUs;
    m_state = ControlState::Idle;

    //全速
    m_isConstSpeed = v0 == vMax;
    m_emptyConfig = false;
}

void ControlAcc::ConfigConstSpeed(int vMax)
{
    m_vMax = vMax;
    m_isConstSpeed = true;
    m_state = ControlState::ConstSpeed;
    m_emptyConfig = false;
}

void ControlAcc::StartAcc()
{
    if(m_emptyConfig)
        return;
    m_totalUs = 0;
    m_currentV = m_v0;
    m_state = ControlState::Acc;
}

void ControlAcc::StartDec()
{
    if(m_emptyConfig)
        return;
    m_totalUs = 0;
    // m_currentV = m_v0;
    m_state = ControlState::Dec;
}

void ControlAcc::Reset()
{
    if(m_emptyConfig)
        return;
    m_totalUs = 0;
    m_state = ControlState::Idle;
}

int ControlAcc::Update(int us)
{
    switch (m_state)
    {
    case ControlState::Idle:
        {
            m_totalUs = 0;   
        }
        break;
    case ControlState::Acc:
        {
            m_totalUs += us;
            if(m_currentV < m_vMax)
                m_currentV = m_v0 + m_totalUs * m_speed;
            if(m_currentV > m_vMax)
            {
                m_currentV = m_vMax;
                m_state = ControlState::ConstSpeed;
            }                
        }
        break;
    case ControlState::ConstSpeed:
        break;
    case ControlState::Dec:
        {
            m_totalUs += us;
            if(m_currentV > m_v0)
                m_currentV = m_vMax - m_totalUs * m_speed;
            if(m_currentV < m_v0)
            {
                m_currentV = m_v0;
                m_state = ControlState::ConstSpeed;
            }                
        }
        break;
    default:
        break;
    }

    return m_currentV;
}
